{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import roboticstoolbox as rtb\n",
    "from spatialmath import *\n",
    "from math import pi\n",
    "import matplotlib.pyplot as plt\n",
    "from matplotlib import cm\n",
    "np.set_printoptions(linewidth=100, formatter={'float': lambda x: f\"{x:8.4g}\" if x > 1e-10 else f\"{0:8.4g}\"})\n",
    "\n",
    "\n",
    "%matplotlib widget"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will instantiate a model of the Puma 560 robot which has well known inertial parameters"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "p560 = rtb.models.DH.Puma560()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and show its configuration in a typical desktop working pose"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "bea7bc9b4ed84f7f998077525bea727b",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "<roboticstoolbox.backend.PyPlot.PyPlot.PyPlot at 0x7f89ccfc2a90>"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p560.plot(p560.qn, block=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The gravity load on its joints is"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([       0,    31.64,    6.035,        0,  0.02825,        0])"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p560.gravload(p560.qn)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and the inertia matrix is"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[   3.659,        0,   0.1006,        0,        0,        0],\n",
       "       [       0,    4.414,   0.3509,        0,  0.00236,        0],\n",
       "       [  0.1006,   0.3509,   0.9378,        0,  0.00148,        0],\n",
       "       [       0,        0,        0,   0.1925,        0, 2.828e-05],\n",
       "       [       0,  0.00236,  0.00148,        0,   0.1713,        0],\n",
       "       [       0,        0,        0, 2.828e-05,        0,   0.1941]])"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p560.inertia(p560.qn)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "the diagonal elements indicate the inertia experienced by the corresponding joints, and the off-diagonal terms couple acceleration of one joint into a disturbance torque on another joint.  The matrix is symmetric.\n",
    "\n",
    "We can study how the inertia at joints 1 and 2  changes as a function of $q_2$ and $q_3$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "N = 100\n",
    "(Q2, Q3) = np.meshgrid(np.linspace(-pi, pi, N), np.linspace(-pi, pi, N))\n",
    "M11 = np.zeros((N,N))\n",
    "M12 = np.zeros((N,N))\n",
    "for i in range(N):\n",
    "    for j in range(N):\n",
    "        M = p560.inertia(np.r_[0, Q2[i,j], Q3[i,j], 0, 0, 0])\n",
    "        M11[i,j] = M[0,0]\n",
    "        M12[i,j] = M[0,1]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and then plot the inertia for joint 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "5537f47c79464e88a240aa4c550580b0",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "fig, ax = plt.subplots(subplot_kw={\"projection\": \"3d\"})\n",
    "surf = ax.plot_surface(Q2, Q3, M11, cmap=cm.coolwarm, linewidth=0, antialiased=False)\n",
    "fig.colorbar(surf, shrink=0.9, aspect=10, pad=0.12)\n",
    "ax.set_xlabel('$q_2$ (rad)')\n",
    "ax.set_ylabel('$q_3$ (rad)')\n",
    "ax.set_zlabel('$M_{11}$ ($kg.m^2$)')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and the coupling inertia between joints 1 and 2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "40d4cbad55e449ec8dac3122bdf38c8c",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "fig, ax = plt.subplots(subplot_kw={\"projection\": \"3d\"})\n",
    "surf = ax.plot_surface(Q2, Q3, M12, cmap=cm.coolwarm, linewidth=0, antialiased=False)\n",
    "fig.colorbar(surf, shrink=0.9, aspect=10, pad=0.12)\n",
    "ax.set_xlabel('$q_2$ (rad)')\n",
    "ax.set_ylabel('$q_3$ (rad)')\n",
    "ax.set_zlabel('$M_{12}$ ($kg.m^2$)')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.6"
  },
  "toc": {
   "base_numbering": 1,
   "nav_menu": {},
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": false,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": false,
   "toc_position": {},
   "toc_section_display": true,
   "toc_window_display": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
